#include "ComController.h"
#include "ComMessage.h"

#define RESPONSELENGTH
#include "MotorControllerCmd.h"

ComController::ComController(ComPort* comPort) {
    m_ComPort = comPort;
    m_ReceivedData = 0;
}

long ComController::SendCommand(int command, short data) {
    SendMessage((char)command, data);
    return GetMessage((char)command);
}

void ComController::SendMessage(char command, short data) {
    char* message = new char[3];
    
    ComMessage::GenerateMessage(command, data, message);
    m_ComPort->Send(message);
   delete message;
}

long ComController::GetMessage(char command)
{
    char responseLength = ResponseLength[command];
    int receivedData = 0;
    if(responseLength != 0)
    {
        char bitShift[4] = {24,16,8,0};
        for(char i=0; i<responseLength; i++)
        {
                char dataByte = m_ComPort->ReceiveChar();
                receivedData |= int(dataByte << bitShift[i]);
        }
    }
    return receivedData;
}

ComController::~ComController() {
}

